#include "rclcpp/rclcpp.hpp"
#include "base_interfaces/msg/armors.hpp"
#include "base_interfaces/msg/game.hpp"
#include "base_interfaces/msg/navigation.hpp"
#include "base_interfaces/msg/bt_aimer.hpp"
#include "base_interfaces/msg/bt_top.hpp"
#include <random>
#include <chrono>
#include <sys/time.h>

using std::placeholders::_1;
using namespace std::chrono_literals;

class BehaviorTreeTest : public rclcpp::Node
{
public:
    BehaviorTreeTest(std::string name) : Node(name)
    {
        Armors = this->create_publisher<base_interfaces::msg::Armors>("Armors", 10);
        // timer_Armors = this->create_wall_timer(50ms, std::bind(&BehaviorTreeTest::callback_Armors, this));
        Game = this->create_publisher<base_interfaces::msg::Game>("Game", 10);
        timer_Game = this->create_wall_timer(50ms, std::bind(&BehaviorTreeTest::callback_Game, this));
        Navigation = this->create_publisher<base_interfaces::msg::Navigation>("Navigation", 10);
        // timer_Navigation = this->create_wall_timer(50ms, std::bind(&BehaviorTreeTest::callback_Navigation, this));

        m_btAimerSubscriber = this->create_subscription<base_interfaces::msg::BtAimer>("BT_shooter", 10, std::bind(&BehaviorTreeTest::subcribeBtAimerCallback, this, _1));
        m_topSub = this->create_subscription<base_interfaces::msg::BtTop>("BT_top", 10, std::bind(&BehaviorTreeTest::subTopCallback, this, _1));
        m_setGimbalAngleSubscriber = this->create_subscription<base_interfaces::msg::GimbalPose>("SetGimbalAngle", 10, std::bind(&BehaviorTreeTest::setGimbalAngle, this, _1));
        m_setGimbal_PitchAngle_YawSpeedSubscriber = this->create_subscription<base_interfaces::msg::GimbalPose>("SetGimbal_YawSpeed_PitchAngle", 10, std::bind(&BehaviorTreeTest::setGimbal_pitchAngle_yawSpeed, this, _1));
        m_getGimbalAngle = this->create_publisher<base_interfaces::msg::GimbalPose>("GetGimbalAngle", 10);
        // timer_getGimbalAngle = this->create_wall_timer(50ms, std::bind(&BehaviorTreeTest::getGimbalAngle, this));

        timer_print = this->create_wall_timer(500ms, std::bind(&BehaviorTreeTest::print, this));
        timeval tv;
        gettimeofday(&tv, NULL);
        m_time_start = tv.tv_sec + 10;
    }

    rclcpp::TimerBase::SharedPtr timer_Armors;
    rclcpp::TimerBase::SharedPtr timer_Game;
    rclcpp::TimerBase::SharedPtr timer_Navigation;
    rclcpp::TimerBase::SharedPtr timer_getGimbalAngle;
    rclcpp::TimerBase::SharedPtr timer_print;

    rclcpp::Publisher<base_interfaces::msg::Armors>::SharedPtr Armors;
    rclcpp::Publisher<base_interfaces::msg::Game>::SharedPtr Game;
    rclcpp::Publisher<base_interfaces::msg::Navigation>::SharedPtr Navigation;
    rclcpp::Subscription<base_interfaces::msg::BtAimer>::SharedPtr m_btAimerSubscriber;
    rclcpp::Subscription<base_interfaces::msg::BtTop>::SharedPtr m_topSub;
    rclcpp::Subscription<base_interfaces::msg::GimbalPose>::SharedPtr m_setGimbalAngleSubscriber;
    rclcpp::Subscription<base_interfaces::msg::GimbalPose>::SharedPtr m_setGimbal_PitchAngle_YawSpeedSubscriber;
    rclcpp::Publisher<base_interfaces::msg::GimbalPose>::SharedPtr m_getGimbalAngle;


    double a = 1;
    double b = 2;
    double c = 3;

    int m_outpost_blood = 1500;
    int m_sentry_blood = 1000;
    int m_bullet_num = 750;
    time_t m_time_start;
    bool m_top_state = false;

    float m_gimbal_angle_pitch = 0;
    float m_gimbal_angle_yaw = 0;
    float m_gimbal_speed_yaw = 0;
    timeval m_last_gimbal_speed_timestamp;

    bool m_navigation_status = false;

    void setGimbalAngle(const base_interfaces::msg::GimbalPose::SharedPtr msg)
    {
        if(m_gimbal_speed_yaw != 0.f)
        {
            RCLCPP_WARN(this->get_logger(), "当前云台速度不为0");
        }
        m_gimbal_angle_pitch = msg->pitch;
        m_gimbal_angle_yaw = msg->yaw;
    }

    void setGimbal_pitchAngle_yawSpeed(const base_interfaces::msg::GimbalPose::SharedPtr msg)
    {
        if(m_gimbal_speed_yaw != 0.f)
        {
            RCLCPP_WARN(this->get_logger(), "当前云台速度不为0");
        }
        m_gimbal_angle_pitch = msg->pitch;
        m_gimbal_speed_yaw = msg->yaw;
        gettimeofday(&m_last_gimbal_speed_timestamp, NULL);
    }

    void getGimbalAngle()
    {
        if(m_gimbal_speed_yaw != 0.f)
        {
            timeval tv;
            gettimeofday(&tv, NULL);
            time_t t1 = (m_last_gimbal_speed_timestamp.tv_sec - m_time_start) * 1e6 + m_last_gimbal_speed_timestamp.tv_usec;
            time_t t2 = (tv.tv_sec - m_time_start) * 1e6 + tv.tv_usec;
            float delta_t = (t2 - t1) / 1e-6;   //单位为usec
            float delta_yaw = m_gimbal_speed_yaw * delta_t;
            m_gimbal_speed_yaw += delta_yaw;
            m_last_gimbal_speed_timestamp = tv;
        }
        base_interfaces::msg::GimbalPose msg;
        msg.pitch = m_gimbal_angle_pitch;
        msg.yaw = m_gimbal_speed_yaw;
        m_getGimbalAngle->publish(msg);
    }

    int getOutpostBlood(){
        return --m_outpost_blood >= 0 ? m_outpost_blood : 0;
    }
    int getSentryBlood(){
        int current_blood = m_sentry_blood;
        if(m_outpost_blood <= 0){
            current_blood = --m_sentry_blood >= 0 ? m_sentry_blood : 0;
        }
        return current_blood;
    }

    void subcribeBtAimerCallback(const base_interfaces::msg::BtAimer::SharedPtr msg)
    {
        if(msg->bullet > 0 && m_bullet_num >= 0)
            m_bullet_num--;
    }

    int getTimeLeft()
    {
        timeval tv;
        gettimeofday(&tv, NULL);
        time_t time_elapsed = tv.tv_sec - m_time_start; //消耗时间
        return 5 * 60 - time_elapsed;   //剩余时间
    }

    void subTopCallback(const base_interfaces::msg::BtTop::SharedPtr msg)
    {
        m_top_state = msg->start;
    }

    void callback_Armors()
    {
        unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
        std::mt19937 generator(seed);
        std::uniform_int_distribution<int> Number(1, 5);

        std::uniform_int_distribution<int> Distance(1, 10);//我不知道你单位是什么，你可以自己调一下随机数的范围
                     
        int armor_number = Number(generator);
        double armor_distance =  (double) Distance(generator);
        auto armorsMsg = base_interfaces::msg::Armors();

        armorsMsg.stamp = now();
        // a = a + 1.0;
        armorsMsg.num = armor_number;
        armorsMsg.distance = armor_distance;

        Armors->publish(armorsMsg);
        //RCLCPP_INFO(this->get_logger(),"Armors已发布,armor_number:%d,  armor_distance:%lf,  time_stamp:%lf",armorsMsg.num,armorsMsg.distance,a);
    }

    void callback_Game()
    {

        auto gameMsg = base_interfaces::msg::Game();

        gameMsg.game_start = getTimeLeft() >= 0 ? true : false;
        gameMsg.outpost_blood = getOutpostBlood();
        gameMsg.sentry_blood = getSentryBlood();
        gameMsg.bullet_num = m_bullet_num;
        gameMsg.time_left = getTimeLeft();
        gameMsg.manual_top = m_top_state;
        gameMsg.game_timestamp = b;
        gameMsg.m_alive = 2;
        gameMsg.enemy_alive = 2;

        Game->publish(gameMsg);
       //RCLCPP_INFO(this->get_logger(),"Game已发布,game_manual_top:%d,  game_timestamp:%lf",gameMsg.manual_top,gameMsg.game_timestamp);

        b++;
    }

    void callback_Navigation()
    {
        auto navigationMsg = base_interfaces::msg::Navigation();

        // navigationMsg.navigation_status = 0;
        navigationMsg.navigation_timestamp = c;

        Navigation->publish(navigationMsg);
        c++;
    }

    void init()
    {
        m_outpost_blood = 1500;
        m_sentry_blood = 1000;
        m_bullet_num = 750;
        m_top_state = false;
        m_gimbal_angle_pitch = 0;
        m_gimbal_angle_yaw = 0;
        m_gimbal_speed_yaw = 0;
        gettimeofday(&m_last_gimbal_speed_timestamp, NULL);
        m_time_start = m_last_gimbal_speed_timestamp.tv_sec;
    }

    void print()
    {
        std::cout << "*******************" << std::endl
                  << "Gimbal Pose:"        << std::endl
                  << "  pitch: " << m_gimbal_angle_pitch << std::endl
                  << "  yaw: " << m_gimbal_angle_yaw   << std::endl
                  << "  yaw speed: " << m_gimbal_speed_yaw << std::endl
                  << "Game State:"         << std::endl
                  << "  outpost blood: " << getOutpostBlood() << std::endl
                  << "  sentry blood: " << getSentryBlood() << std::endl
                  << "  bullet num: " << m_bullet_num << std::endl
                  << "  time left: " << getTimeLeft() << std::endl
                  << "  top state: " << m_top_state << std::endl;
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    /*产生一个节点*/
    auto node = std::make_shared<BehaviorTreeTest>("BehaviorTreeTest");
    /* 运行节点，并检测退出信号*/
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}